﻿// LaserPlaneCalibrator
// Laser plane calibration functionality
// 
// FLMScan - Featured by FabLab Munich

using System;
using System.Collections.Generic;
using FLMScan.Geometry;
using FLMScan.OpenCV;
using log4net;

namespace FLMScan.WorldCoordinateReconstruction.Riedmaier
{
    internal class LaserPlaneCalibrator
    {
        private ILog logger;

        private List<FlmPoint> ledWorldPositions;
        private CameraSensor cameraSensor;                

        private FlmMatrix cameraMatrix;
        private FlmMatrix distortionCoefficientsMatrix;
        private FlmPoint principalPoint;
        
        private List<FlmPoint> laserLinePoints;
        private List<FlmPlane> ledPlanes;
        
        public LaserPlaneCalibrator(List<FlmPoint> ledWorldPositions, CameraSensor sensor, List<double> distortionCoefficients)
        {
            logger = LogManager.GetLogger(typeof(LaserPlaneCalibrator));

            this.ledWorldPositions = ledWorldPositions;
            cameraSensor = sensor;
            laserLinePoints = new List<FlmPoint>();
            ledPlanes = new List<FlmPlane>();

            distortionCoefficientsMatrix = new FlmMatrix(1, 4);

            for (int i = 0; i < distortionCoefficientsMatrix.Columns; i++)
            {
                distortionCoefficientsMatrix[0, i] = distortionCoefficients[i];
            }

            cameraMatrix = new FlmMatrix(3, 3);            
            
            // TBD: properly set primary point
            // unclear: what's the unit for the primary point?
            // millimeters on the CCD chip or pixels?
            // width = 4.536 mm / 2 = 2.268
            // height = 3.416 mm / 2 = 1.708
            // width = 1600 / 2 = 800
            // height = 1200 / 2 = 600
            // principalPoint = new FlmPoint(2.268,1.708);
            principalPoint = new FlmPoint(sensor.PrimaryPointInPixelsX,sensor.PrimaryPointInPixelsY);

            cameraMatrix[0, 0] = sensor.FocalLengthInPixelsX;
            cameraMatrix[0, 1] = 0.0;
            cameraMatrix[0, 2] = principalPoint.X;

            cameraMatrix[1, 0] = 0.0;
            cameraMatrix[1, 1] = sensor.FocalLengthInPixelsY;
            cameraMatrix[1, 2] = principalPoint.Y;

            cameraMatrix[2, 0] = 0.0;
            cameraMatrix[2, 1] = 0.0;
            cameraMatrix[2, 2] = 1.0;

            if (logger.IsDebugEnabled)
            {
                cameraMatrix.Log("cameraMatrix: ");
            }
            
        }

        public void ProcessMeasurement(PlaneCalibratorMeasurement measurement)
        {
            int upperLeftX = (int)measurement.Led1Position2d.X;
            int upperLeftY = (int)measurement.Led1Position2d.Y;
            int lowerRightX = (int)measurement.Led1Position2d.X;
            int lowerRightY = (int)measurement.Led1Position2d.Y;
            
            // calculate boundaries of LED rectangle
            List<FlmPoint> ledPositions = new List<FlmPoint>();
            ledPositions.Add(measurement.Led1Position2d);
            ledPositions.Add(measurement.Led2Position2d);
            ledPositions.Add(measurement.Led3Position2d);
            ledPositions.Add(measurement.Led4Position2d);

            foreach (FlmPoint ledPosition in ledPositions)
            {
                if (ledPosition.X < upperLeftX)
                    upperLeftX = (int)ledPosition.X;
                if (ledPosition.X > lowerRightX)
                    lowerRightX = (int)ledPosition.X;

                if (ledPosition.Y < upperLeftY)
                    upperLeftY = (int)ledPosition.Y;
                if (ledPosition.Y > lowerRightY)
                    lowerRightY = (int)ledPosition.Y;
            }

            // TBD: rather calculate the actual intersection point
            //measurement.UpperlaserLinePosition2d.Y = upperLeftY;
            //measurement.LowerLaserLinePosition2d.Y = lowerRightY;

            //
            // calculate intersection points of laser line with LED rectangle
            //

            // check whether laser line overlaps LED rectangle vertically
            //if ( (measurement.UpperlaserLinePosition2d.Y <= upperLeftY) 
            //    && (measurement.LowerLaserLinePosition2d.Y >= lowerRightY) )
            {
                // check whether laser line is located between left and right LEDs
                //if ((measurement.UpperlaserLinePosition2d.X >= upperLeftX)
                //    && (measurement.UpperlaserLinePosition2d.X <= lowerRightX))
                {
                    FlmPoint led1Position = new FlmPoint(upperLeftX, upperLeftY);
                    FlmPoint led2Position = new FlmPoint(lowerRightX, upperLeftY);
                    FlmPoint led3Position = new FlmPoint(lowerRightX, lowerRightY);
                    FlmPoint led4Position = new FlmPoint(upperLeftX, lowerRightY);

                    List<FlmPoint> sortedLedPositions = new List<FlmPoint>();
                    sortedLedPositions.Add(led1Position);
                    sortedLedPositions.Add(led2Position);
                    sortedLedPositions.Add(led3Position);
                    sortedLedPositions.Add(led4Position);

                    // determine LED plane in world coordinates 
                    // from 3D LED locations and captured 2D LED locations
                    FlmPlane ledPlane = GetWorldCoordinateLedPlane(sortedLedPositions);

                    ledPlanes.Add(ledPlane);

                    if (logger.IsDebugEnabled)
                    {
                        ledPlane.Log("ledPlane: ");
                    }

                    // calculate 3D intersection points of laser line with LED rectangle
                    // i.e. calculate location on plane

                    FlmMatrix originalLaserLineIntersectionPoints = new FlmMatrix(2, 2);
                    originalLaserLineIntersectionPoints[0, 0] = measurement.UpperlaserLinePosition2d.X;
                    originalLaserLineIntersectionPoints[0, 1] = measurement.UpperlaserLinePosition2d.Y;
                    originalLaserLineIntersectionPoints[1, 0] = measurement.LowerLaserLinePosition2d.X;
                    originalLaserLineIntersectionPoints[1, 1] = measurement.LowerLaserLinePosition2d.Y;

                    // TBD: reintegrate undistort 
                    // TBD: removed because it results in undesired values of undistorted points
                    
                    // FlmMatrix undistortedPoints = CalibrationWrapperOpenCV.Undistort(originalLaserLineIntersectionPoints, cameraMatrix, distortionCoefficientsMatrix);

                    //FlmPoint undistortedIntersectionScreen1 = new FlmPoint(undistortedPoints[0, 0] - principalPoint.X, undistortedPoints[0, 1] - principalPoint.Y);
                    //FlmPoint undistortedIntersectionScreen2 = new FlmPoint(undistortedPoints[1, 0] - principalPoint.X, undistortedPoints[1, 1] - principalPoint.Y);

                    FlmPoint undistortedIntersectionScreen1 = new FlmPoint(measurement.UpperlaserLinePosition2d.X - principalPoint.X, measurement.UpperlaserLinePosition2d.Y - principalPoint.Y);
                    FlmPoint undistortedIntersectionScreen2 = new FlmPoint(measurement.LowerLaserLinePosition2d.X - principalPoint.X, measurement.LowerLaserLinePosition2d.Y - principalPoint.Y);

                    if (logger.IsDebugEnabled)
                    {
                        originalLaserLineIntersectionPoints.Log("originalLaserLineIntersectionPoints: ");
                        undistortedIntersectionScreen1.Log("undistortedIntersectionScreen1: ");
                        undistortedIntersectionScreen2.Log("undistortedIntersectionScreen2: ");
                    }

                    double xRatio1 = undistortedIntersectionScreen1.X / cameraSensor.FocalLengthInPixelsX;
                    double yRatio1 = undistortedIntersectionScreen1.Y / cameraSensor.FocalLengthInPixelsY;
                    double xRatio2 = undistortedIntersectionScreen2.X / cameraSensor.FocalLengthInPixelsX;
                    double yRatio2 = undistortedIntersectionScreen2.Y / cameraSensor.FocalLengthInPixelsY;

                    double s1 = (-1) * ledPlane.D / (ledPlane.A * xRatio1 + ledPlane.B * yRatio1 + ledPlane.C);
                    double s2 = (-1) * ledPlane.D / (ledPlane.A * xRatio2 + ledPlane.B * yRatio2 + ledPlane.C);                    

                    FlmPoint laserLineIntersectionWorld1 = new FlmPoint(s1 * xRatio1, s1 * yRatio1, s1);
                    FlmPoint laserLineIntersectionWorld2 = new FlmPoint(s2 * xRatio2, s2 * yRatio2, s2);

                    if (logger.IsDebugEnabled)
                    {
                        laserLineIntersectionWorld1.Log("laserLineIntersectionWorld1: ");
                        laserLineIntersectionWorld2.Log("laserLineIntersectionWorld2: ");                        
                    }
                    
                    // store 3D laser line intersection points for current measurement                    
                    laserLinePoints.Add(laserLineIntersectionWorld1);
                    laserLinePoints.Add(laserLineIntersectionWorld2);
                }
            }
        }

        public bool CanCalculateCalibrationPlane()
        {
            return (laserLinePoints.Count > 0);
        }

        public List<FlmPoint> LaserLinePoints
        {
            get { return laserLinePoints; }
        }

        public List<FlmPlane> LedPlanes
        {
            get { return ledPlanes; }
        }

        public FlmPlane GetCalibrationPlane()
        {            
            // calculate laser plane from stored laser line vectors
            FlmPlane laserPlane = FitPlane(laserLinePoints);

            if (logger.IsDebugEnabled)
            {
                laserPlane.Log("laserPlane: ");                
            }

            return laserPlane;            
        }        

        private FlmPlane GetWorldCoordinateLedPlane(List<FlmPoint> webcamPositions)
        {
            List<FlmPoint> webcamWorldPositions = Reconstruct3DPosition(ledWorldPositions, webcamPositions);
            System.Diagnostics.Debug.Assert(webcamWorldPositions.Count > 3);
            // TBD: ugh, this is ugly, but ok for the moment
            bool normalizeDistance = false;
            FlmPlane plane = new FlmPlane(webcamWorldPositions[0], webcamWorldPositions[1], webcamWorldPositions[3], normalizeDistance);
            return plane;
        }

        static FlmPlane FitPlane(List<FlmPoint> laserPlanePoints)
        {
            FlmMatrix matA = new FlmMatrix(laserPlanePoints.Count, 3);
            FlmMatrix matB = new FlmMatrix(laserPlanePoints.Count, 1);

            int count = 0;
            foreach (FlmPoint point in laserPlanePoints)
            {
                point.Log("plane fitting point: ");

                matA[count, 0] = point.X;
                matA[count, 1] = point.Y;
                matA[count, 2] = point.Z;

                matB[count, 0] = -1;

                ++count;
            }

            FlmMatrix matX = MatrixWrapperOpenCV.Solve(matA, matB);

            double scale = 1.0 / Math.Sqrt(Math.Pow(matX[0, 0], 2) + Math.Pow(matX[1, 0], 2) + Math.Pow(matX[2, 0], 2));
            double a = matX[0, 0] * scale;
            double b = matX[1, 0] * scale;
            double c = matX[2, 0] * scale;
            double d = 1 * scale;

            if ((Math.Abs(a) > Math.Abs(b) && a < 0) || (Math.Abs(b) > Math.Abs(a) && b < 0))
            {
                a *= -1;
                b *= -1;
                c *= -1;
                d *= -1;
            }

            FlmPlane plane = new FlmPlane(a, b, c, d);
            return plane;
        }

        // 
        // see http://opencv.willowgarage.com/documentation/camera_calibration_and_3d_reconstruction.html
        // for details about camera calibration and reconstruction
        // 
        List<FlmPoint> Reconstruct3DPosition(List<FlmPoint> ledWorldPositions, List<FlmPoint> webcamPositions)
        {
            FlmMatrix webcamPoints = new FlmMatrix(2, 4);
            FlmMatrix ledWorldPoints = new FlmMatrix(3, 4);

            int count = 0;
            foreach (FlmPoint position in webcamPositions)
            {
                webcamPoints[0, count] = position.X;
                webcamPoints[1, count] = position.Y;
                ++count;
            }

            if (logger.IsDebugEnabled)
            {
                webcamPoints.Log("webcamPoints: ");
            }

            count = 0;
            foreach (FlmPoint position in ledWorldPositions)
            {
                ledWorldPoints[0, count] = position.X;
                ledWorldPoints[1, count] = position.Y;
                ledWorldPoints[2, count] = position.Z;
                ++count;
            }

            if (logger.IsDebugEnabled)
            {
                ledWorldPoints.Log("ledWorldPoints: ");
            }

            FlmMatrix translationVector;
            FlmMatrix rotationMatrix;
            CalibrationWrapperOpenCV.FindExtrinsicCameraParams2(ledWorldPoints, webcamPoints, cameraMatrix, distortionCoefficientsMatrix, out rotationMatrix, out translationVector);

            if (logger.IsDebugEnabled)
            {
                translationVector.Log("translationVector: ");
                rotationMatrix.Log("rotationMatrix: ");
            }

            // create translation matrix            
            FlmMatrix shiftMatrix = new FlmMatrix(3, 4);

            // write x translation in first row, y translation in second row, z translation in last row
            for (int i = 0; i < 3; i++)
            {
                double value = translationVector[i, 0];
                shiftMatrix[i, 0] = value;
                shiftMatrix[i, 1] = value;
                shiftMatrix[i, 2] = value;
                shiftMatrix[i, 3] = value;
            }

            FlmMatrix transform = MatrixWrapperOpenCV.MatrixMultiply(rotationMatrix, ledWorldPoints, shiftMatrix);

            // ah, that's interesting: expectation would have been that we 
            // multiply here the ledPositions against the inverse matrix
            // instead we simply retrieve the world points that were transformed
            // against the matrix

            if (logger.IsDebugEnabled)
            {
                transform.Log("transform: ");
            }

            List<FlmPoint> positions3d = new List<FlmPoint>();

            for (int i = 0; i < ledWorldPositions.Count; i++)
            {
                double x = transform[0, i];
                double y = transform[1, i];
                double z = transform[2, i];

                FlmPoint worldPosition = new FlmPoint(x, y, z);
                positions3d.Add(worldPosition);
            }

            return positions3d;
        }
    }
}
